%% dyn_param_linearization.m
% @brief: convert dynamic equation to linear form `tau=W·P`
%         where tau is obtained by dynamic equation Eq.(2-25), with shape(7, 1)
% @notes: W is regression matrix, a nonlinear matrix only relative to [q, qd, qdd], with shape(7, 13x7)
%         P is standard set of dynamic parameter, with shape(13x7, 1)
%         (Refer to `Study on Parameter Identification for Rokae XB4`)
% @notes: Unit:mm(Nmm) is used throughout the project.

%% PARAMETER
% number of dynamic parameter for each joint
% {m,mc1,mc2,mc3,Ioxx,Ioyy,Iozz,Ioxy,Ioxz,Ioyz,Ia,fv,fc}
pnum_per_joint = 13;  
% number of total dynamic parameter for 7-joint robot
pnum_sum = pnum_per_joint * 7;

%% INSTANTIATION
%  e.g. [t1, t2]' = A * b = [A1, A2] * [b1, b2]'
%  where t1 = A11 * b1 + A12 * b2
%        t2 = A21 * b1 + A22 * b2
%        A1 = [A11, A21]', A2 = [A12, A22]' denote column vectors
%  to figure out A1: we can assign {b1=1, b2=0} in matrix A
%                A2: we can assign {b1=0, b2=1} in matrix A
%  because variable b1 and b2 are linearly independent, no non-linear term like b1*b2 in equation
Q_ = zeros(pnum_sum, 1);	% index matrix
W = sym(zeros(7, pnum_sum));	% regression matrix
for i = 1:pnum_sum
    Q_ = zeros(pnum_sum, 1);
	% index of joint-1, e.g. k1 = [0, 1, 2, 3, 4, 5, 6] for 7-joint robot
    k1 = fix((i-1) / pnum_per_joint);
	% index of param-1, e.g. k2 = [0, 1, 2, ..., 12] for standard 13 params	
    k2 = mod(i-1, pnum_per_joint);		
    
    Q_(i) = 1;
    
    if (k2>=1 && k2<=3)		% for k2 = {mc1, mc2, mc3}
        Q_(k1 * pnum_per_joint + 1) = 1;   % for m
  
        W(:,i) = subs(linkTAU, ...
		              {'m1', 'mc11', 'mc12', 'mc13', 'Io111', 'Io122', 'Io133', 'Io112', 'Io113', 'Io123', 'Ia1', 'fv1', 'fc1', ...
                       'm2', 'mc21', 'mc22', 'mc23', 'Io211', 'Io222', 'Io233', 'Io212', 'Io213', 'Io223', 'Ia2', 'fv2', 'fc2', ...
                       'm3', 'mc31', 'mc32', 'mc33', 'Io311', 'Io322', 'Io333', 'Io312', 'Io313', 'Io323', 'Ia3', 'fv3', 'fc3', ...
                       'm4', 'mc41', 'mc42', 'mc43', 'Io411', 'Io422', 'Io433', 'Io412', 'Io413', 'Io423', 'Ia4', 'fv4', 'fc4', ...
                       'm5', 'mc51', 'mc52', 'mc53', 'Io511', 'Io522', 'Io533', 'Io512', 'Io513', 'Io523', 'Ia5', 'fv5', 'fc5', ...
                       'm6', 'mc61', 'mc62', 'mc63', 'Io611', 'Io622', 'Io633', 'Io612', 'Io613', 'Io623', 'Ia6', 'fv6', 'fc6', ...
					   'm7', 'mc71', 'mc72', 'mc73', 'Io711', 'Io722', 'Io733', 'Io712', 'Io713', 'Io723', 'Ia7', 'fv7', 'fc7'}, ...
					  {Q_(1), Q_(2), Q_(3), Q_(4), Q_(5), Q_(6), Q_(7), Q_(8), Q_(9), Q_(10), Q_(11), Q_(12), Q_(13), ...
					   Q_(14), Q_(15), Q_(16), Q_(17), Q_(18), Q_(19), Q_(20), Q_(21), Q_(22), Q_(23), Q_(24), Q_(25), Q_(26), ...
					   Q_(27), Q_(28), Q_(29), Q_(30), Q_(31), Q_(32), Q_(33), Q_(34), Q_(35), Q_(36), Q_(37), Q_(38), Q_(39), ...
					   Q_(40), Q_(41), Q_(42), Q_(43), Q_(44), Q_(45), Q_(46), Q_(47), Q_(48), Q_(49), Q_(50), Q_(51), Q_(52), ...
					   Q_(53), Q_(54), Q_(55), Q_(56), Q_(57), Q_(58), Q_(59), Q_(60), Q_(61), Q_(62), Q_(63), Q_(64), Q_(65), ...
				       Q_(66), Q_(67), Q_(68), Q_(69), Q_(70), Q_(71), Q_(72), Q_(73), Q_(74), Q_(75), Q_(76), Q_(77), Q_(78), ...
					   Q_(79), Q_(80), Q_(81), Q_(82), Q_(83), Q_(84), Q_(85), Q_(86), Q_(87), Q_(88), Q_(89), Q_(90), Q_(91)}) ...
			     - W(:, k1 * pnum_per_joint + 1);
    else
        W(:,i) = subs(linkTAU, ...
					  {'m1', 'mc11', 'mc12', 'mc13', 'Io111', 'Io122', 'Io133', 'Io112', 'Io113', 'Io123', 'Ia1', 'fv1', 'fc1', ...
                       'm2', 'mc21', 'mc22', 'mc23', 'Io211', 'Io222', 'Io233', 'Io212', 'Io213', 'Io223', 'Ia2', 'fv2', 'fc2', ...
                       'm3', 'mc31', 'mc32', 'mc33', 'Io311', 'Io322', 'Io333', 'Io312', 'Io313', 'Io323', 'Ia3', 'fv3', 'fc3', ...
                       'm4', 'mc41', 'mc42', 'mc43', 'Io411', 'Io422', 'Io433', 'Io412', 'Io413', 'Io423', 'Ia4', 'fv4', 'fc4', ...
                       'm5', 'mc51', 'mc52', 'mc53', 'Io511', 'Io522', 'Io533', 'Io512', 'Io513', 'Io523', 'Ia5', 'fv5', 'fc5', ...
                       'm6', 'mc61', 'mc62', 'mc63', 'Io611', 'Io622', 'Io633', 'Io612', 'Io613', 'Io623', 'Ia6', 'fv6', 'fc6', ...
					   'm7', 'mc71', 'mc72', 'mc73', 'Io711', 'Io722', 'Io733', 'Io712', 'Io713', 'Io723', 'Ia7', 'fv7', 'fc7'}, ...
				      {Q_(1), Q_(2), Q_(3), Q_(4), Q_(5), Q_(6), Q_(7), Q_(8), Q_(9), Q_(10), Q_(11), Q_(12), Q_(13), ...
					   Q_(14), Q_(15), Q_(16), Q_(17), Q_(18), Q_(19), Q_(20), Q_(21), Q_(22), Q_(23), Q_(24), Q_(25), Q_(26), ...
					   Q_(27), Q_(28), Q_(29), Q_(30), Q_(31), Q_(32), Q_(33), Q_(34), Q_(35), Q_(36), Q_(37), Q_(38), Q_(39), ...
					   Q_(40), Q_(41), Q_(42), Q_(43), Q_(44), Q_(45), Q_(46), Q_(47), Q_(48), Q_(49), Q_(50), Q_(51), Q_(52), ...
					   Q_(53), Q_(54), Q_(55), Q_(56), Q_(57), Q_(58), Q_(59), Q_(60), Q_(61), Q_(62), Q_(63), Q_(64), Q_(65), ...
					   Q_(66), Q_(67), Q_(68), Q_(69), Q_(70), Q_(71), Q_(72), Q_(73), Q_(74), Q_(75), Q_(76), Q_(77), Q_(78), ...
					   Q_(79), Q_(80), Q_(81), Q_(82), Q_(83), Q_(84), Q_(85), Q_(86), Q_(87), Q_(88), Q_(89), Q_(90), Q_(91)});
    end

    disp(['<INFO> Param No.', num2str(i), ' SUBSTITUTED!!']);
end
